#include "fdc1004.h"
#include <string.h>

#define TAG "FDC1004"

// Private structure for device context
typedef struct {
    i2c_port_t i2c_port;
    uint8_t i2c_address;
} fdc1004_dev_t;

// Private function declarations
static esp_err_t fdc1004_write_register(fdc1004_handle_t handle, uint8_t reg, uint16_t value);
static esp_err_t fdc1004_read_register(fdc1004_handle_t handle, uint8_t reg, uint16_t *value);
static esp_err_t fdc1004_read_registers(fdc1004_handle_t handle, uint8_t start_reg, uint8_t *data, size_t len);

fdc1004_handle_t fdc1004_init(const fdc1004_config_t *config)
{
    if (config == NULL) {
        ESP_LOGE(TAG, "Config is NULL");
        return NULL;
    }

    // Configure I2C
    i2c_config_t i2c_conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = config->sda_pin,
        .scl_io_num = config->scl_pin,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = config->i2c_freq,
    };

    esp_err_t ret = i2c_param_config(config->i2c_port, &i2c_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C parameter config failed: %s", esp_err_to_name(ret));
        return NULL;
    }
    else
    {
        ESP_LOGI(TAG, "I2C parameter config success");
    }
    ret = i2c_driver_install(config->i2c_port, I2C_MODE_MASTER, 0, 0, 0);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C driver install failed: %s", esp_err_to_name(ret));
        return NULL;
    }

    // Allocate device context
    fdc1004_dev_t *dev = (fdc1004_dev_t *)malloc(sizeof(fdc1004_dev_t));
    if (dev == NULL) {
        ESP_LOGE(TAG, "Memory allocation failed");
        i2c_driver_delete(config->i2c_port);
        return NULL;
    }

    dev->i2c_port = config->i2c_port;
    dev->i2c_address = config->i2c_address;

    // Verify device communication by reading manufacturer ID
    uint16_t manufacturer_id;
    ret = fdc1004_read_manufacturer_id(dev, &manufacturer_id);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read manufacturer ID");
        free(dev);
        i2c_driver_delete(config->i2c_port);
        return NULL;
    }

    if (manufacturer_id != 0x5449) {
        ESP_LOGE(TAG, "Invalid manufacturer ID: 0x%04X", manufacturer_id);
        free(dev);
        i2c_driver_delete(config->i2c_port);
        return NULL;
    }

    ESP_LOGI(TAG, "FDC1004 initialized successfully");
    return (fdc1004_handle_t)dev;
}

void fdc1004_deinit(fdc1004_handle_t handle)
{
    if (handle == NULL) {
        return;
    }

    fdc1004_dev_t *dev = (fdc1004_dev_t *)handle;
    i2c_driver_delete(dev->i2c_port);
    free(dev);
}

esp_err_t fdc1004_reset(fdc1004_handle_t handle)
{
    if (handle == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    // Set reset bit in FDC configuration register
    return fdc1004_write_register(handle, FDC1004_FDC_CONF, (1 << 15));
}

esp_err_t fdc1004_configure_measurement(fdc1004_handle_t handle, 
                                       fdc1004_measurement_t measurement_id,
                                       fdc1004_channel_t positive_channel,
                                       fdc1004_channel_t negative_channel,
                                       uint8_t capdac_value,
                                       fdc1004_rate_t rate)
{
    if (handle == NULL || measurement_id > FDC1004_MEASUREMENT_4) {
        return ESP_ERR_INVALID_ARG;
    }

    // Validate CAPDAC value
    if (capdac_value > 31) {
        return ESP_ERR_INVALID_ARG;
    }

    // Calculate configuration register value
    uint16_t config_value = 0;
    
    // Set positive channel (CHA)
    config_value |= (positive_channel & 0x07) << 13;
    
    // Set negative channel (CHB)
    config_value |= (negative_channel & 0x07) << 10;
    
    // Set CAPDAC value
    config_value |= (capdac_value & 0x1F) << 5;

    // Write to appropriate configuration register
    uint8_t config_reg = FDC1004_CONF_MEAS1 + measurement_id;
    esp_err_t ret = fdc1004_write_register(handle, config_reg, config_value);
    if (ret != ESP_OK) {
        return ret;
    }

    // Set measurement rate in FDC configuration register
    uint16_t fdc_conf;
    ret = fdc1004_read_register(handle, FDC1004_FDC_CONF, &fdc_conf);
    if (ret != ESP_OK) {
        return ret;
    }

    fdc_conf &= ~(0x03 << 10);  // Clear rate bits
    fdc_conf |= (rate & 0x03) << 10;

    return fdc1004_write_register(handle, FDC1004_FDC_CONF, fdc_conf);
}

esp_err_t fdc1004_trigger_measurement(fdc1004_handle_t handle, fdc1004_measurement_t measurement_id)
{
    if (handle == NULL || measurement_id > FDC1004_MEASUREMENT_4) {
        return ESP_ERR_INVALID_ARG;
    }

    uint16_t fdc_conf;
    esp_err_t ret = fdc1004_read_register(handle, FDC1004_FDC_CONF, &fdc_conf);
    if (ret != ESP_OK) {
        return ret;
    }

    // Clear repeat bit and all measurement trigger bits
    fdc_conf &= ~(0x1F0);
    
    // Set trigger bit for specified measurement
    fdc_conf |= (1 << (7 - measurement_id));

    return fdc1004_write_register(handle, FDC1004_FDC_CONF, fdc_conf);
}

esp_err_t fdc1004_trigger_repeated_measurements(fdc1004_handle_t handle, uint8_t measurement_mask)
{
    if (handle == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    uint16_t fdc_conf;
    esp_err_t ret = fdc1004_read_register(handle, FDC1004_FDC_CONF, &fdc_conf);
    if (ret != ESP_OK) {
        return ret;
    }

    // Set repeat bit and measurement trigger bits
    fdc_conf &= ~(0x1F0);
    fdc_conf |= (1 << 8);  // Set REPEAT bit
    fdc_conf |= ((measurement_mask & 0x0F) << 4);

    return fdc1004_write_register(handle, FDC1004_FDC_CONF, fdc_conf);
}

esp_err_t fdc1004_is_measurement_done(fdc1004_handle_t handle, fdc1004_measurement_t measurement_id, bool *is_done)
{
    if (handle == NULL || is_done == NULL || measurement_id > FDC1004_MEASUREMENT_4) {
        return ESP_ERR_INVALID_ARG;
    }

    uint16_t fdc_conf;
    esp_err_t ret = fdc1004_read_register(handle, FDC1004_FDC_CONF, &fdc_conf);
    if (ret != ESP_OK) {
        return ret;
    }

    *is_done = (fdc_conf & (1 << (3 - measurement_id))) != 0;
    return ESP_OK;
}

esp_err_t fdc1004_read_measurement(fdc1004_handle_t handle, fdc1004_measurement_t measurement_id, 
                                  fdc1004_measurement_result_t *result)
{
    if (handle == NULL || result == NULL || measurement_id > FDC1004_MEASUREMENT_4) {
        return ESP_ERR_INVALID_ARG;
    }

    // Calculate register addresses for this measurement
    uint8_t msb_reg = FDC1004_MEAS1_MSB + (measurement_id * 2);
    uint8_t lsb_reg = msb_reg + 1;

    // Read measurement data
    uint16_t msb, lsb;
    esp_err_t ret = fdc1004_read_register(handle, msb_reg, &msb);
    if (ret != ESP_OK) {
        return ret;
    }

    ret = fdc1004_read_register(handle, lsb_reg, &lsb);
    if (ret != ESP_OK) {
        return ret;
    }

    // Combine into 24-bit value (LSB register only uses bits 15:8)
    int32_t raw_value = ((int32_t)msb << 8) | ((lsb >> 8) & 0xFF);
    
    // Convert to capacitance (pF) using formula from datasheet
    // Capacitance (pF) = raw_value / 2^19 + Coffset
    // For now, we assume Coffset = 0 (no CAPDAC)
    float capacitance = (float)raw_value / 524288.0f;  // 2^19 = 524288

    result->raw_value = raw_value;
    result->capacitance_pf = capacitance;
    result->measurement_id = measurement_id;

    // Clear the DONE flag by reading the measurement
    uint16_t fdc_conf;
    ret = fdc1004_read_register(handle, FDC1004_FDC_CONF, &fdc_conf);
    if (ret == ESP_OK) {
        fdc_conf &= ~(1 << (3 - measurement_id));  // Clear DONE bit
        fdc1004_write_register(handle, FDC1004_FDC_CONF, fdc_conf);
    }

    return ESP_OK;
}

esp_err_t fdc1004_set_offset_calibration(fdc1004_handle_t handle, fdc1004_channel_t channel, float offset_pf)
{
    if (handle == NULL || channel > FDC1004_CIN4 || offset_pf < -16.0f || offset_pf > 16.0f) {
        return ESP_ERR_INVALID_ARG;
    }

    // Convert pF to fixed-point format (5 bits integer, 11 bits fractional)
    int16_t offset_int = (int16_t)(offset_pf * 2048.0f);  // 2^11 = 2048
    uint16_t reg_value = (uint16_t)offset_int;

    uint8_t offset_reg = FDC1004_OFFSET_CAL_CIN1 + channel;
    return fdc1004_write_register(handle, offset_reg, reg_value);
}

esp_err_t fdc1004_set_gain_calibration(fdc1004_handle_t handle, fdc1004_channel_t channel, float gain)
{
    if (handle == NULL || channel > FDC1004_CIN4 || gain < 0.0f || gain > 4.0f) {
        return ESP_ERR_INVALID_ARG;
    }

    // Convert gain to fixed-point format (2 bits integer, 14 bits fractional)
    uint16_t gain_int = (uint16_t)(gain * 16384.0f);  // 2^14 = 16384
    uint16_t reg_value = gain_int;

    uint8_t gain_reg = FDC1004_GAIN_CAL_CIN1 + channel;
    return fdc1004_write_register(handle, gain_reg, reg_value);
}

esp_err_t fdc1004_read_manufacturer_id(fdc1004_handle_t handle, uint16_t *manufacturer_id)
{
    if (handle == NULL || manufacturer_id == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    return fdc1004_read_register(handle, FDC1004_MANUFACTURER_ID, manufacturer_id);
}

esp_err_t fdc1004_read_device_id(fdc1004_handle_t handle, uint16_t *device_id)
{
    if (handle == NULL || device_id == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    return fdc1004_read_register(handle, FDC1004_DEVICE_ID, device_id);
}

// Private functions
static esp_err_t fdc1004_write_register(fdc1004_handle_t handle, uint8_t reg, uint16_t value)
{
    fdc1004_dev_t *dev = (fdc1004_dev_t *)handle;
    
    uint8_t write_buf[3] = {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)};
    
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev->i2c_address << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(cmd, write_buf, sizeof(write_buf), true);
    i2c_master_stop(cmd);
    
    esp_err_t ret = i2c_master_cmd_begin(dev->i2c_port, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Write register 0x%02X failed: %s", reg, esp_err_to_name(ret));
    }
    
    return ret;
}

static esp_err_t fdc1004_read_register(fdc1004_handle_t handle, uint8_t reg, uint16_t *value)
{
    fdc1004_dev_t *dev = (fdc1004_dev_t *)handle;
    
    // First write the register address
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev->i2c_address << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_stop(cmd);
    
    esp_err_t ret = i2c_master_cmd_begin(dev->i2c_port, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Set register address 0x%02X failed: %s", reg, esp_err_to_name(ret));
        return ret;
    }
    
    // Then read the register value
    uint8_t read_buf[2];
    cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev->i2c_address << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, read_buf, sizeof(read_buf), I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    
    ret = i2c_master_cmd_begin(dev->i2c_port, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret == ESP_OK) {
        *value = (read_buf[0] << 8) | read_buf[1];
    } else {
        ESP_LOGE(TAG, "Read register 0x%02X failed: %s", reg, esp_err_to_name(ret));
    }
    
    return ret;
}